/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file XMLtoData.cxx
	\brief class implemenation XML parser

	\author Jack Elston
	$Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "PeteMAVParser.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
	extern bool verbose;
	CommandQueue commandQueue;
 ZigbeeQueue ZigbeeQueue;
/*<-End Global Variables-->*/

MAVParser::MAVParser(String name) : ThreadedPipe(name)
{
	xb_init = false;
	my_addr = 0;
	last_addr_req.stamp();
}

void MAVParser::setAddress(uint16_t xb_addr)
{
	my_addr = xb_addr;
	Client * me = shmPtr->clients.getClient(0);
	in_addr address;
	address.s_addr = inet_addr("6.1.0.0");
	address.s_addr |= ((uint32_t)xb_addr) << 24;
	address.s_addr |= ((uint32_t)(xb_addr & 0xFF00)) << 8;
	shmPtr->clients.addAddr(me,address);
}

void MAVParser::update() 
{
	uint16_t MavNum;
	int val, maxFD;
	fd_set readFDs, writeFDs;									 // fd sets
	struct timeval timeout;

	if(!xb_init) {
		currentStamp.stamp();
		if((currentStamp - last_addr_req ) > (ADDR_REQ_PERIOD * SEC2MICRO) ) {
			ZigbeeQueue.requestAddress((uint8_t *)fifo_msg.data);
			fifo_msg.size = 8;
			writeFifo(OUTBOARD_TX);
			last_addr_req.stamp();
		}
	}

	FD_ZERO(&readFDs);									// zero read fd set
	FD_ZERO(&writeFDs);									// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("MAVParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0, zigbee_size= 0;
			uint8_t byte[1]={0};

			n=readFifo(OUTBOARD_RX);

			if(n>0) {
				ZigbeeQueue.push((uint8_t*)fifo_msg.data,n);
				
				while( !ZigbeeQueue.APIneedMoreBytes() ) {
					if(ZigbeeQueue.APIisValid()){
						//cout << "	good API_Csum\n";
						MavNum= ZigbeeQueue.getMavNum();
						//cout << MavNum << endl; 

						zigbee_size= ZigbeeQueue.size();
						//cout << "zigbee_size: " << zigbee_size << endl;


						if(!xb_init) {
							uint16_t length; 
							uint16_t address;

							ZigbeeQueue.popByte();
							length = (uint16_t)(ZigbeeQueue.popByte() << 8);
							length |= (uint16_t)(ZigbeeQueue.popByte());
							
							if(length == 7)
								if(ZigbeeQueue.popByte() == 0x88)
									if(ZigbeeQueue.popByte() != 0x0)
										if(ZigbeeQueue.popByte() == 'M')
											if(ZigbeeQueue.popByte() == 'Y')
												if(ZigbeeQueue.popByte() == 0) {
													address = (uint16_t)(ZigbeeQueue.popByte() << 8);
													address |= (uint16_t)(ZigbeeQueue.popByte());
													setAddress(address);
													xb_init = true;
												}
						}

						else {

							for(int i= 0; i< (zigbee_size-API_TX_HEADER_SIZE-API_CSUM_SIZE); i++){
								//do not copy API header bytes or API CSum byte

								byte[0]= ZigbeeQueue.getByte(i+API_TX_HEADER_SIZE);
								commandQueue.push(byte,1);
							}

							ZigbeeQueue.popCommand();

							while( !commandQueue.needMoreBytes() ) {
								if (commandQueue.isValid() ){
									//cout << "	 good CMD_Csum\n";

									if(verbose){
										cout << "MAVParser::update() - valid command: " << MavNum << endl;
									}

									Command cmd = commandQueue.popCommand();
									cmd.copyToBuf((uint8_t*)fifo_msg.data);

									if(verbose)
										printf("[%x]\n", CMD_CMD(fifo_msg.data));

									fifo_msg.src.s_addr = inet_addr("6.1.0.0");
									fifo_msg.src.s_addr |= ((uint32_t)MavNum) << 24;
									fifo_msg.src.s_addr |= ((uint32_t)(MavNum & 0xFF00)) << 8;
									fifo_msg.size = cmd.size();
									writeFifo(INBOARD_TX);
								}//end of: if(commandQueue.isValid())

								else {
									if(verbose)
										cout << "bad CMD_Csum" << endl;
									commandQueue.pop();
								}
							}//end of: while( !commandQueue.needMoreBytes() 
						}
					}//end of: if(ZigbeeQueue.APIisValid())
					
					else{
						if(verbose)
							cout << "bad API_Csum" << endl;
						ZigbeeQueue.popByte();
					}
				}//end of: while( !ZigbeeQueue.APIneedMoreBytes() 
			}//end of: if(n>0)
		}//end of: if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);

			//create a fake flightplan
			//n= 208+5;

			uint16_t destination_address;
			uint8_t temp1[n];
			uint8_t temp2[n + API_TX_HEADER_SIZE + API_CSUM_SIZE]; //include room for API header and Csum
			uint8_t temp3[n + API_TX_HEADER_SIZE + API_CSUM_SIZE]; //include room for API header and Csum

			memcpy(temp1, fifo_msg.data, n); // elstonj - hack


			if(temp1[0] == CMD_FP){//sending a flight plan

				//create a fake flightplan
				//temp1[0]= CMD_FP;
				//temp1[1]= 0x01;
				//temp1[2]= n-5;
				//for(int i=3; i<n-2; i++){
				//	temp1[i]= i-2;
				//}
				//temp1[n-2]= 0xA9;
				//temp1[n-1]= 0xC3;

				//cout << "Flight Plan\n";
				//cout << "Original Packet\n";
				//for(int i= 0; i< n; i++){
				//	printf("%.2X ",temp1[i]);
				//}
				//cout << endl;

				Command partialFP;
				uint16_t number_bytes;
				uint8_t number_WPs;
				uint8_t number_packets;
				uint8_t payload_size= 0;

				number_bytes= n - CMD_PACKET_SIZE;
				number_WPs= number_bytes/BYTES_PER_WP;

				if(number_WPs % WPs_PER_PACKET == 0)
					number_packets= number_WPs/WPs_PER_PACKET;
				else
					number_packets= (uint8_t)floor(number_WPs/WPs_PER_PACKET) +1;

				//cout << "number_bytes= " << number_bytes << endl;
				//printf("number_WPs= %i \n",number_WPs);
				//printf("number_packets= %i \n",number_packets);
				//cout << endl;

				//for(int i= 1; i<= 1; i++){
				for(int i= 1; i<= number_packets; i++){
					//chop up original packet into smaller packets
					if(number_bytes/(i * WPs_PER_PACKET * BYTES_PER_WP) >= 1)
						payload_size= WPs_PER_PACKET * BYTES_PER_WP;
					else
						payload_size= number_bytes - (WPs_PER_PACKET * BYTES_PER_WP*(i-1));

					//printf("payload_size= %i \n",payload_size);
					CMD_CMD(temp2)= CMD_FP;
					CMD_SEQ(temp2)= i;
					CMD_NUM(temp2)= payload_size;

					for(int k= CMD_HEADER_SIZE; k< payload_size+CMD_HEADER_SIZE; k++){
						temp2[k]= temp1[k + (i-1)*WPs_PER_PACKET * BYTES_PER_WP];
					}

					partialFP.copyFromBuf((uint8_t*)(temp2));
					partialFP.setCheckSum();
					partialFP.copyToBuf(temp2);

					memcpy(temp3+API_TX_HEADER_SIZE, temp2, payload_size+CMD_HEADER_SIZE+CMD_CSUM_SIZE);

					//Insert API header
					destination_address= (uint16_t)(fifo_msg.dest.s_addr >> 24); 
					destination_address |= (uint16_t)((fifo_msg.dest.s_addr >> 8) & 0xFF00); 
					//cout << destination_address << endl;
					ZigbeeQueue.APIheaders(temp3,destination_address,payload_size+CMD_HEADER_SIZE+CMD_CSUM_SIZE);
			
					//Insert API CSum
					ZigbeeQueue.APIsetCheckSum(temp3,payload_size+CMD_HEADER_SIZE+CMD_CSUM_SIZE);

					//for(int i=0; i<payload_size+5+9; i++){
				//	printf(" %.2X",temp3[i]); 
					//}
					//cout << endl;

					memcpy(fifo_msg.data, temp3, payload_size+API_TX_HEADER_SIZE+
							API_CSUM_SIZE+CMD_HEADER_SIZE+CMD_CSUM_SIZE);

					fifo_msg.size = payload_size + CMD_HEADER_SIZE + CMD_CSUM_SIZE
						+ API_TX_HEADER_SIZE + API_CSUM_SIZE;

					//cout << "fifo_msg.size= " << fifo_msg.size << endl;
					writeFifo(OUTBOARD_TX);

					sleep(1);
				}
			}

			else{
				memcpy(temp3+API_TX_HEADER_SIZE, temp1, n);//leave room for API header bytes

				//Insert API header
				//destination_address= 0xFFFF;//broadcast address
				destination_address= (uint16_t)(fifo_msg.dest.s_addr >> 24); 
				destination_address |= (uint16_t)((fifo_msg.dest.s_addr >> 8) & 0xFF00); 
				//cout << destination_address << endl;
				ZigbeeQueue.APIheaders(temp3,destination_address,n);
			
				//Insert API CSum
				ZigbeeQueue.APIsetCheckSum(temp3,n);

				//for(int i=0; i<n+9; i++){
			//	printf(" %.2X",temp3[i]); 
				//}
				//cout << endl;

				memcpy(fifo_msg.data, temp3, n + API_TX_HEADER_SIZE + API_CSUM_SIZE );

				fifo_msg.size = n + API_TX_HEADER_SIZE + API_CSUM_SIZE;
				writeFifo(OUTBOARD_TX);
			}
		}
	}
}

